# 基于OpenCV的相机标定 并且输出

import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt

# 准备参考点, like (0, 0, 0), (1, 0, 0), (2, 0, 0) ....,(6, 5, 0)
objp = np.zeros((6 * 9, 3), np.float32)
objp[:,:2] = np.mgrid[0 : 9, 0 : 6].T.reshape(-1, 2)

# 定义两个空数组来保存这些点 
objpoints = [] # 3D真实的点
imgpoints = [] # 2D的点

# 列出校准图像
images = glob.glob('camera_cal/calibration*.jpg')

# 遍历并且搜索浏览角
for idx, fname in enumerate(images):
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 寻找浏览叫
    ret, corners = cv2.findChessboardCorners(gray, (9,6), None)

    if ret == True:
        objpoints.append(objp)
        imgpoints.append(corners)

        # 可视化 并保存
        cv2.drawChessboardCorners(img, (9,6), corners, ret)
        write_name = 'C_cal/corners_found'+str(idx)+'.jpg'
        cv2.imwrite(write_name, img)
        cv2.imshow('img', img)
        cv2.waitKey(500)

cv2.destroyAllWindows()